% Dynamic_Params
global m g l
global Jx Jy Jz I J
global a1 a2 a3 
global b1 b2 b3
global kt kd 
m = 2;
g = 9.81; 
l = 5;
Jx = 15.67e-3; 
Jy = 15.67e-3;
Jz = 28.34e-3;
I = 6.01e-5; 
J = [Jx 0  0;
     0  Jy 0
     0  0  Jz];
a1 = (Jy-Jz)/Jx;
a2 = (Jz-Jx)/Jy; 
a3 = (Jx-Jy)/Jz;
b1 = l/Jx;
b2 = l/Jy;
b3 = 1/Jz;
kt = 3.13e-5; kd = 7.5e-7;

% Gain_params
global c1 c2 c3 c4 c5 c6
global c7 c8 c9 c10 c11 c12
%% 初始位置1对应参数
% c1 = 10; c2 = 15;  % PHI
% c3 = 8; c4 = 1;  % THETA
% c5 = 8; c6 = 1;  % PSI
% c7 = 8; c8 = 1;  % X
% c9 = 8; c10 = 1; % Y
% c11 = 10; c12 = 5; % Z
%% 初始化值2对应参数
% c1 = 4; c2 = 1;  % PHI
% c3 = 5; c4 = 4;  % THETA % c4 = 3、4
% c5 = 4; c6 = 1;  % PSI
% c7 = 4; c8 = 1;  % X
% c9 = 4; c10 = 1; % Y
% c11 = 4; c12 = 1; % Z

% %% 悬停高度一定
% c1 = 8; c2 = 2;  % PHI
% c3 = 8; c4 = 1;  % THETA
% c5 = 8; c6 = 1;  % PSI
% c7 = 9; c8 = 1;  % X
% c9 = 8; c10 = 1; % Y
% c11 = 7; c12 = 1; % Z
%% test
c1 = 6; c2 = 1;  % PHI
c3 = 6; c4 = 1;  % THETA
c5 = 6; c6 = 1;  % PSI
c7 = 6; c8 = 1;  % X
c9 = 6; c10 = 1; % Y
c11 = 6; c12 = 1; % Z

% Simulation_params
dt = 0.01;
Tf = 50;
Quadrotor_Num = 4;